package environment;
import java.awt.Color;
import java.awt.Graphics;
import java.awt.Point;
import java.awt.Polygon;
import java.awt.Rectangle;

/**
 *  Robot
 *  Robot class that wanders randomly in the environment until it finds
 *  a target box it can push
 *
 *  Modified from robot.java
 *  - Removed machine learning details
 *  - Add box detection
 * 
 *  @author Toar
 *  
 */
import java.util.*;

import communication.Message;
import communication.MessageManager;
import communication.Message.MessageType;
import data.Logger;
import environment.Sonar.Loc;

public class Robot extends Thread implements EnvObjInterface {
	private String id;
	private Box detectedBox = null;
	protected Box myBox = null; //protected Box myBox = new Box("fake",0,0);
	private Capabilities myCapabilities;
	
	/** Running 5 tests
	 * 1. Single robot fails while pushing
	 * 2. Cooperation - helper fails while approaching
	 * 3. Cooperation - helper fails during transport
	 * 4. Cooperation - coord fails while helper is approaching
	 * 5. Cooperation - coord fails during transportation
	 */
	public static boolean test[] = {true,true,true,true,true};
	private static String  testBoxIDs[] = new String[5];
	
	protected ArrayList<String> ignoreBoxes = new ArrayList<String>();
	
	/** Robot can be in multiple states 
	 *  hence, the boolean flags best serve this 
	 **/
	protected boolean isPushing = false;
	protected boolean isWaitingForHelp = false;
	protected boolean isHelpArrived = false;
	protected boolean isHelping = false;
	
	protected boolean isFailed = false;
	
	protected String allyID = "";
	protected int currentAction = -1;
	
	private int waitForCost = 0;
	private int myCost = -9999;
	private int waitCount = 0;
	private int WAIT_COUNT_THRESHOLD = 500;
	
	/** Fault tolerance values */
	private boolean isWaitingForAllyAliveMsg = false;
	private int MAXWaitTime = 500;
	private int WaitForAllyAliveMsg = 0;
	private int previousLocCounter = 0;
	private Point previousLoc = new Point(0,0), currentLoc;
	private int waitForForcePointMsg = 0;
	private boolean isWaitingForForcePointMsg = false;
	private int pushCounter = 0, step_stamp = 0;
	
	/** 
	 * Need a queue data structure for Messages
	 * LinkedList acts like a double ended queue
	 * Use methods addFirst, removeFirst
	 */
	private LinkedList<Message> messageQueue = new LinkedList<Message>();
	
	/** Robot dimension **/
	private int width = 30;
	private int height = 30;
	private double x;				// center x
	private double y;				// center y
	double angle = 175;
	
	private Goal goal;
	private final int SIGHT = 100; // how far can the robot see an object
	private final int SONAR_ANGLE = 30;
	
	protected int velocity = 10;
	private int turnVelocity = 8;
	
	private Rectangle bounds;
	private Color robotColor = Color.black;		// configurable
	private Color sonarColor = Color.yellow;
	
	/** Robot & sonar points **/
	/** 
	 * Robot body points, P0 = front, P1 = sideLeft, P2 sideRight
	 * P3 = rearRight, P4 = rearLeft
	 */
	Point p0,p1,p2,p3,p4;
	Point p5,p6,p7;       // sonar points
	
	// 0 default, 1 and above is configurable, see updateRobotPoints()
	public int drawType = 0;
	
	/** failure probability **/
	private float [] failChance = new float[3];
	public enum FailState { WANDER, HELP, PUSH }
	
	/** Simulating goal status, set goalKnown to true if you want to bypass this **/
	protected boolean goalKnown = true;
	
	protected Sonar sonar;
	
	/** Bumper boolean flags **/
	private boolean leftSideBumperHit = false, rightSideBumperHit = false;
	private boolean leftRearBumperHit = false, rightRearBumperHit = false;
	private boolean frontBumperHit = false;
	
	/** direction constants **/
	static final int RIGHT = 2, LEFT = 1, FORWARD = 7, BACKWARD = 6;
	
	private double lastWinnerMessage = 0;
	
	public static double partialFailChance = 0.5;
	
	/**
	 * Constructor for robot, including goal
	 * @param myID String for robot id/name
	 * @param x Robot's center x
	 * @param y Robot's center y
	 * @param g Robot's goal
	 */
	public Robot(String myID, int x, int y, Goal g)
	{
		id = myID;
		this.bounds = new Rectangle(x,y,width,height);
		this.x = (int)bounds.getCenterX();
		this.y = (int)bounds.getCenterY();
		goal = g;
		sonar = new Sonar(this);
		updateRobotPoints();
		myCapabilities = new Capabilities(true,true,true,true,true);
	}
	
	/**
	 * Constructor for robot, including goal and its dimension
	* @param myID String for robot id/name
	 * @param x Robot's center x
	 * @param y Robot's center y
	 * @param w Robot's width
	 * @param h Robot's height
	 * @param g Robot's goal
	 */
	public Robot(String myID, int x, int y, int w, int h, Goal g)
	{
		id = myID;
		this.width = w;
		this.height = h;
		this.bounds = new Rectangle(x,y,width,height);
		this.x = (int)bounds.getCenterX();
		this.y = (int)bounds.getCenterY();
		goal = g;
		sonar = new Sonar(this);
		updateRobotPoints();
		myCapabilities = new Capabilities(true,true,true,true,true);
	}
	
	/**
	 * Constructor for robot, including goal
	 * @param myID String for robot id/name
	 * @param x Robot's center x
	 * @param y Robot's center y
	 * @param g Robot's goal
	 * @param cap Robot's capability
	 */
	public Robot(String myID, int x, int y, Goal g, Capabilities cap)
	{
		this.id = myID;
		this.bounds = new Rectangle(x,y,width,height);
		this.x = (int)bounds.getCenterX();
		this.y = (int)bounds.getCenterY();
		this.goal = g;
		this.myCapabilities = cap;
	}
	
	/**
	 * Constructor for robot
	 * @param myID String for robot id/name
	 * @param x Robot's center x
	 * @param y Robot's center y
	 */
	public Robot(String myID, int x, int y)
	{
		id = myID;
		this.bounds = new Rectangle(x,y,width,height);
		this.x = (int)bounds.getCenterX();
		this.y = (int)bounds.getCenterY();
		updateRobotPoints();
		myCapabilities = new Capabilities(true,true,true,true,true);
	}

	/**
	 * Gets the robot id/name
	 */
	public String getID() {
		return id;
	}
	
	/**
	 * Get robot's width
	 */
	public double getWidth() {
		return width;
	}

	/**
	 * Get robot's height
	 */
	public double getHeight() {
		return height;
	}
	
	/**
	 * Get robot's bounds
	 */
	public Rectangle getBounds()
	{
		return bounds;
	}

	/**
	 * Returns global coordinate for center x of robot
	 */
	public int getX() {
		return (int)this.x;
	}

	/**
	 * Returns global coordinate for center y of robot
	 */
	public int getY() {
		return (int)this.y;
	}
	
	/**
	 * Get the robot's angle
	 */
	public double getAngle() {
		return angle;
	}
	
	/**
	 * Checks whether or not box is being pushed to the goal
	 * @param b
	 * @return True if box reaches goal area
	 */
	public boolean isDonePushing(Box b) 
	{
		return goal.contains(b);
	}
	
	/**
	 * isTouching
	 * @param x - x axis
	 * @param y - y axis
	 * @return true if point is within robot's bounds
	 * 			false otherwise
	 */
	public boolean isTouching(int x, int y) 
	{
		return this.bounds.contains(x, y);
	}
	
	/**
	 * isTouchg
	 * @return true if object is touching robot's bounds
	 * 			false otherwise
	 */
	public boolean isTouching(EnvObjInterface eObj) 
	{
		if(eObj.getID().startsWith("r"))
			return this.bounds.intersects(eObj.getBounds());
		return eObj.isTouching(this);
	}
	
	/**
	 * Robots are NOT pushable
	 */
	public boolean isPushable() {
		return false;
	}

	/**
	 * Method to check if robot is outside of screen
	 * @return true if robot is out
	 * 			false otherwise
	 */
	public boolean isRobotOutsideScreen() {
		if(ScreenConstants.isOutOfScreen(p0) || 
				ScreenConstants.isOutOfScreen(p1) ||
				ScreenConstants.isOutOfScreen(p2) || 
				ScreenConstants.isOutOfScreen(p3) ||
				ScreenConstants.isOutOfScreen(p4))
			return true;
		return false;
	}
	
	/**
	 * @DEPRECATED - Replaced with validateRobotPoints(a)
	 * @param a --
	 * @return --
	 */
	public boolean isValidMove(ArrayList<EnvObjInterface> a)
	{
		if(isRobotOutsideScreen())
			return false;
		
		for(EnvObjInterface obj : a)
		{
			if(obj.getID() != this.id && isTouching(obj) &&
					( !obj.isPushable()  ||
					  (obj.isPushable() && ignoreBoxes.contains(obj.getID()))
					 )
				)
							
			{
				return false;
			}
		}
		return true;
	}
	
	/**
	 * Validate robot points, checks for robot hitting the wall
	 * and also robot hitting obstacles. Pushable boxes are not considered
	 * obstacles. Boolean flags for bumper hit are set through this method.
	 * @param a List of objects within the world
	 * @return True if all points are valid (no bumper hits!) false otherwise
	 */
	public boolean validateRobotPoints(ArrayList<EnvObjInterface> a)
	{
		/** Check for robot hitting the wall **/
		this.frontBumperHit = ScreenConstants.isOutOfScreen(p0);
		this.leftSideBumperHit = ScreenConstants.isOutOfScreen(p1);
		this.rightSideBumperHit = ScreenConstants.isOutOfScreen(p2);
		this.leftRearBumperHit = ScreenConstants.isOutOfScreen(p4);
		this.rightRearBumperHit = ScreenConstants.isOutOfScreen(p3);
		
		/** 
		 * Check for robot hitting any obstacles 
		 * NOTE: pushable boxes are not considered obstacles!
		 *       Once touching, robot will attempt to push the box 
		 **/
		for(EnvObjInterface obj : a)
		{
			if(obj.getID() != this.id && 
					( !obj.isPushable()  || (obj.isPushable() && ignoreBoxes.contains(obj.getID())))
			  )
			{
				this.frontBumperHit |= obj.getBounds().contains(p0);
				this.leftSideBumperHit |= obj.getBounds().contains(p1);
				this.rightSideBumperHit |= obj.getBounds().contains(p2);
				this.leftRearBumperHit |= obj.getBounds().contains(p4);
				this.rightRearBumperHit |= obj.getBounds().contains(p3);
			}
		}
		return !(frontBumperHit || leftSideBumperHit || rightSideBumperHit || 
				leftRearBumperHit || rightRearBumperHit);
	}
	
	/**
	 * Obstacle detection
	 * @param a - array list of all objects in the environment
	 * @return true - if sonars detect obstacle
	 * 			false - otherwise
	 */
	public boolean isObstacleDetected(ArrayList<EnvObjInterface> a)
	{
		if(sonar.isObstacleDetected(a))
		{
			return true;
		}		
		return false;
	}
	
	/**
	 * Box detection
	 * @param a - array list of all objects in the environment
	 * @return true - if pushable object, "box", is detected
	 * 			false - otherwise
	 */
	public boolean isBoxDetected(ArrayList<EnvObjInterface> a)
	{
		for(EnvObjInterface obj : a)
		{
			if(obj.getID() != this.id && obj.isPushable() && !ignoreBoxes.contains(obj.getID()) && sonar.isSingleObstacleDetected(obj)  )
			{
				this.detectedBox = (Box) obj;
				isHelping = false;
				
	
				return true;
			}
		}
		
		return false;
	}

	/**
	 * Sets the robot's failure chance. Default is 0.
	 * @param p Chance of failure (0 to 1)
	 * @param state Fail state
	 */
	public void setFailureChance(float p, FailState state) 
	{
		this.failChance[state.ordinal()] = p;
	}
	
	/**
	 * Sets the robot's velocity (points/cycle)
	 * Minimum velocity is 10
	 * @param velocity (points/cycle)
	 */
	public void setVelocity(int velocity)
	{
		if(velocity < 10)
			velocity = 10;
		this.velocity = velocity;
	}
	
	/**
	 * Sets the robot's angle
	 * @param angle In degrees
	 */
	public void setAngle(double angle)
	{
		this.angle = angle;
	}
	
	/**
	 * Sets robot color
	 * @param c - color
	 */
	public void setColor(Color c)
	{
		robotColor = c;
	}
	
	/**
	 * Sets the goal for the robot
	 * @param g - goal
	 */
	public void setGoal(Goal g)
	{
		goal = g;
	}
	
	/**
	 * Update the points of the robots for drawing
	 */
	public void updateRobotPoints() 
	{
		int xLoc = width/2, xLoc2 = width/4, yLoc = height/2, yLoc2 = height;
		
		switch(drawType)
		{
			case 1:
				xLoc = width/8;
				xLoc2 = width/2;
				break;
			/** In Progress */
			case 2:
				xLoc = width/2;
				xLoc2 = width/4;
				yLoc = +height;
				yLoc2 = 0;
				break;
			default:
				break;
		}
		
		/** for the robot **/
		p0=new Point(0,-yLoc);
		p1=new Point(-xLoc,0);
		p2=new Point(xLoc,0);
		p3=new Point(xLoc2,yLoc);
		p4=new Point(-xLoc2,yLoc);
				
		/** sonar points **/
		p5=new Point(0,-height/2 - SIGHT);
		p6=new Point((int)(SIGHT * Math.sin(Math.toRadians(SONAR_ANGLE))),(int)-(SIGHT * Math.cos(Math.toRadians(SONAR_ANGLE))));
		p7=new Point((int)-(SIGHT * Math.sin(Math.toRadians(SONAR_ANGLE))),(int)-(SIGHT * Math.cos(Math.toRadians(SONAR_ANGLE))));
		
		/** convert to global coordinates **/
		p0 = frame2To1(p0);
		p1 = frame2To1(p1);
        p2 = frame2To1(p2);
        p3 = frame2To1(p3);
        p4 = frame2To1(p4);
        p5 = frame2To1(p5);
        p6 = frame2To1(p6);
        p7 = frame2To1(p7);
        
        /** update the sonars */
        sonar.updatePoints(p0, p1, p2, p5, p6, p7);
	}
	
	/**
	 * Draws the robot
	 */
	public void draw(Graphics g) 
	{
		updateRobotPoints();

        /** draw the robot **/
        g.setColor(robotColor);
        g.drawLine( p1.x, p1.y, p0.x, p0.y);
        g.drawLine( p2.x, p2.y, p0.x, p0.y);
        g.drawLine( p2.x, p2.y, p3.x, p3.y);
        g.drawLine( p3.x, p3.y, p4.x, p4.y);
        g.drawLine( p4.x, p4.y, p1.x, p1.y);
        
        /** draw sonars **/
        if(!isFailed) {
        g.setColor(sonarColor);
        g.drawLine( p5.x, p5.y, p0.x, p0.y);
        g.drawLine( p6.x, p6.y, p2.x, p2.y);
        g.drawLine( p7.x, p7.y, p1.x, p1.y);
        }
        
        Polygon p = new Polygon();
        
        p.addPoint(p1.x, p1.y);
        p.addPoint(p0.x, p0.y);
        p.addPoint(p2.x, p2.y);
        p.addPoint(p3.x, p3.y);
        p.addPoint(p4.x, p4.y);
        
        /** fill the insides of the robot with robotColor **/
        g.setColor(robotColor);
        g.fillPolygon(p);
        
        /** label the robot with its name, label with FAIL if it has failed **/
        g.setColor(Main.stringColor);
        
        if(this.isFailed) g.drawString(id + " FAIL", (int)x, (int)y);
        else g.drawString(id, (int)x, (int)y);
        
        g.setColor(Color.green);        
	}
	
	/**
	 * Obsolete method
	 */
	public void move(int howManyX, int howManyY, ArrayList<EnvObjInterface> a) 
	{
		this.x += howManyX;
		this.y += howManyY;
		this.bounds.setBounds((int)x, (int)y, width, height);
		if(!isValidMove(a))
		{
			this.x -= 2*howManyX;
			this.y -= 2*howManyY;
			this.bounds.setBounds((int)x, (int)y, width, height);
		}
	}
	
	/** Method to instantly move the robots on the indicated
	 *  x and y axes.
	 * @param x - x axis
	 * @param y - y axis
	 */
	public void moveTo(int x, int y) 
	{
		this.x = x;
		this.y = y;
		this.bounds.setBounds(x, y, width, height);
	}

	/**
	 * Commands the robot to push a box towards goal.
	 * Assume that robot is touching box
	 * @param b - box to push
	 */
	public void pushToGoal(Box b)
	{
		int action = 0;
		double goalAngle = b.getGoalAngle();
		double err = 5;
		
		/** Figure out the position to push, relative to the goal **/
		if(goalAngle <= 0 + err || goalAngle >= 360 - err)
			action = 7;
		else if(goalAngle <= 180 + err && goalAngle >= 180 - err)
			action = 6;
		else if(goalAngle <= 90 && goalAngle >= 0)
			action = 5;
		else if(goalAngle <= 180 && goalAngle >= 90)
			action = 1;
		else if(goalAngle <= 270 && goalAngle >= 180)
			action = 5;
		else if(goalAngle >= 270)
			action = 4;
		
		/**
		 * need this block of code once we get to 2 robots
		 */
		if(b.numberOfRobotsToPush == 2)
		{
			askForHelp(b);
			
			/** If robot is alone, try to push from middle **/
			if(!isHelpArrived && !isHelping)
			{
				if(action == 5 || action == 4)
					action = 7;
				if(action == 1 || action == 2)
					action = 6;
				Point p = b.getForcePoint(action);
				moveTo(p.x, p.y);
				this.angle = b.getAngle();
				if(action == 6 || action == 1 || action == 2)
					this.angle = (180 + angle) %360;
				return;
			}
			
			/** wait for GO_TO_FORCE_POINT message **/
			if(isHelping && currentAction < 0)
			{
				this.waitForForcePointMsg++;
			
				/** If the robot has waited long enough for first GO_TO_FORCE_POINT
				 *  and it has not received it, COORDINATOR has died. New election.
				 */
				if(this.waitForForcePointMsg > this.MAXWaitTime)
				{
					
					Logger.log("DETECT " + id + " COORD_IS_DEAD: " + allyID);
					
					/** Reset values and assume the coordinator position **/
					this.waitForForcePointMsg = 0;
					this.previousLocCounter = 0;
					this.allyID = "";
					this.isHelpArrived = false;
					this.isHelping = false;
					this.isWaitingForHelp = true;
					b.isBeingPushed = false;
					this.waitCount = 0;
					return;
				}	
				
				
				return;
			}
			
			/** If robot is the helper, let the coordinator tell him
			 *  where to push
			 */
			if(isHelping)
			{
				action = currentAction;
			}
			/** Will go to the following loop if not helping
			 *  meaning that this robot is the coordinator
			 */
			else if(action == 6)
			{
				/** STRAIGHT **/
				// need to push from 1
				action = 1;
				
				/** Tell the helper robot to push from point 2 **/
				MessageManager.sendGoToForcePointMessage(id, allyID, b.getID(), 2);
			}
			else if(action == 7)
			{
				/** STRAIGHT **/
				// need to push from 5
				action = 5;
				
				/** Tell the helper robot to push from point 4 **/
				MessageManager.sendGoToForcePointMessage(id, allyID, b.getID(), 4);
				// or action = 4;
			}
			else if(action == 5 || action == 2)
			{
				/** TURNING LEFT **/
				// need to push from 5
				action = 5;
				
				/** Tell the helper robot to push from point 2 **/
				MessageManager.sendGoToForcePointMessage(id, allyID, b.getID(), 2);
			}
			else if(action == 1 || action == 4)
			{
				/** TURNING RIGHT **/
				// need to push from 1
				action = 1;
				
				/** Tell the helper robot to push from point 4 **/
				MessageManager.sendGoToForcePointMessage(id, allyID, b.getID(), 4);
			}
		} /** END 2 ROBOTS **/
		
		/** If the assigned force point is occupied by a robot(most likely to be dead)
		 *  occupy the other force point adjacent to the point assigned
		 */
		if(PointIsInADeadRobot(myBox.getForcePoint(action)) || (this.step_stamp > 0) )
		{
			if(this.step_stamp == 0) this.step_stamp = 50;
			this.step_stamp--;
			
			if(action == 1 || action == 2)
			{
				action = 6;
			}
			else if(action == 5 || action == 4)
			{
				action = 7;
			}
		}
		
		/** ACTUAL PUSHING from the forcepoint **/
		
		myBox.setStartBeingPushed();
		
		/** Make other robots ignore box when it is being pushed **/
		if(!this.isWaitingForAllyAliveMsg)
		{
			b.isBeingPushed = true;
		}
		
		/** Move the robot to the Force Point of box */
		Point p1 = b.getForcePoint(action);
		moveTo(p1.x, p1.y);
		
		/** Apply force to the assigned point on the box */
		b.exertForce(action, 10);
		
		/** Calculate new position of the box */
		b.boxResponse();	
		
		/** Move the robot to the NEW Force Point of box after pushing*/
		Point p2 = b.getForcePoint(action);
		moveTo(p2.x, p2.y);                                         
		
		/** keep track the number of pushes **/
		previousLocCounter++;
		
		/** 
		 * If the box didn't move the last 50 iterations(pushes) ally might be dead,
		 * check by sending ARE_YOU_ALIVE message.
		 **/
		if(previousLocCounter % 50 == 0 || this.isWaitingForAllyAliveMsg)
		{
			
			/** if 50 iterations ago, position still the same, send ARE_YOU_ALIVE **/
			if(previousLoc.x == p2.x && previousLoc.y == p2.y && !this.isWaitingForAllyAliveMsg)
			{
				MessageManager.sendAreYouAliveMessage(this.id, this.allyID, b.getID(), false);
				this.isWaitingForAllyAliveMsg = true;
				return;
			}
			/** if we're waiting for the ally's reply **/
			else if(this.isWaitingForAllyAliveMsg && this.WaitForAllyAliveMsg <= this.MAXWaitTime)
			{
				this.WaitForAllyAliveMsg++;
				return;
			}
			/** we've waited enough for the ally to reply, assume it to be dead.
			 *  Call a new ally, assume the coordinator position.
			 */
			else if(this.WaitForAllyAliveMsg > this.MAXWaitTime)
			{
				/** ally is dead for sure **/
				Logger.log("DETECT " + id + " ALLY_IS_DEAD: " + allyID);
				this.isWaitingForAllyAliveMsg = false;
				this.WaitForAllyAliveMsg = 0;
				this.previousLocCounter = 0;
				this.allyID = "";
				this.isHelpArrived = false;
				this.isHelping = false;
				this.isWaitingForHelp = true;
				b.isBeingPushed = false;
				
				/** setting the wait count equal to 0, enables askforhelp method to start new
				 	election **/
				this.waitCount = 0;
				return;
			}
			
			previousLoc = new Point(p2.x, p2.y);
		}
		
		/** Angle where the robots are facing */
		this.angle = b.angle;
		if(action == 6 || action == 1 || action == 2)
			this.angle = (180 + angle) %360;
		
		/** If this robot is in helping mode, must always wait for forcepoint msg **/
		if(isHelping)
		{
			//currentAction = -1;
		}
	}

	/**
	 * Printing robot id and location
	 */
	public void printStats() {
		Logger.log("Robot " + id + " is at " + this.x + ", " + this.y);
		System.err.println("-----");			
		System.err.println("Robot " + id + " done pushing " + myBox.getID());
		System.err.println("is pushing? " + isPushing);
		System.err.println("is isHelping? " + isHelping);
		System.err.println("is isHelpArrived? " + isHelpArrived);
		System.err.println("is isWaitingForHelp? " + isWaitingForHelp);
		System.err.println("-----");			
	}
	
	/**
	 * Helper for draw() - to figure out robot's corner
	 * @param p
	 * @return
	 */
	private Point frame2To1(Point p)
	{
        double newAngle = angle/180*(Math.PI);
        
        double x=p.x;
        double y=p.y;
        
        double newX = x * Math.cos(newAngle) - y * Math.sin(newAngle) + this.x;
        double newY = x * Math.sin(newAngle) + y * Math.cos(newAngle) + this.y;
        return new Point((int)newX, (int)newY);
    }
	
	/**
	 * Push the box to the intended goal
	 */
	private void doPushing()
	{
		/** NOT DONE PUSHING YET - Perform transportation **/
		if(!isDonePushing(myBox) && myBox.isPushable())
		{	
			/** If we have no path to the goal**/
			if(myBox.goalPath == null)
			{
				myBox.goalPath = PathCalculator.getGoalPath(myBox, this.goal);
			}
			
			/** FAILURE DURING PUSHING **/
			//if(Main.getRandomNumber(id) > this.failChance[FailState.PUSH.ordinal()])
			/*if(Main.RUN_FALSE_TOLERANCE_TESTS && 
					( myBox.getID().equals("b1") && test[0] && myBox.round2 > 0 && (Main.rounds - myBox.round2 > 50))  ||
					( myBox.getID().equals("b2") && test[1] && myBox.round1 > 0 && (Main.rounds - myBox.round1 > 10))  ||
					( myBox.getID().equals("b3") && test[2] && myBox.round2 > 0 && (Main.rounds - myBox.round2 > 30))
			)*/
			if(Main.RUN_FALSE_TOLERANCE_TESTS && 
					( myBox.getID().equals("b1") && test[0] && myBox.round2 > 0 && (Main.rounds - myBox.round2 > 50))  ||
					( myBox.getID().equals("b2") && test[1] && myBox.round1 > 0 && (Main.rounds - myBox.round1 > 10) && allyID != "")  ||
					( myBox.getID().equals("b3") && test[3] && myBox.round2 > 0 && (Main.rounds - myBox.round2 > 30) && !isHelping) ||
					( myBox.getID().equals("b4") && test[4] && myBox.round2 > 0 && (Main.rounds - myBox.round2 > 100) && isHelping)
			)
			{
				if( myBox.getID().equals("b1") ) 
					test[0] = false;
				if( myBox.getID().equals("b2") ) 
					test[1] = false;
				if( myBox.getID().equals("b3") ) 
					test[3] = false;
				if( myBox.getID().equals("b4") ) 
					test[4] = false;
				
				if(myBox.numberOfRobotsToPush == 1) 
				{
					myBox.isBeingPushed = false;
				}
				
				/** 50 percent chance of partial failure **/
				if(Main.getRandomNumber(id + " CHECK PARTIAL FAILURE PUSH ") < partialFailChance)
				{
					if(myBox.numberOfRobotsToPush == 1) 
					{
						MessageManager.sendBoxNotifyMessage(id, myBox);
					}
					else
					{
						/** send dying message to ALLY **/
						if(!isHelpArrived && !isHelping)
							MessageManager.sendImDying(id, allyID, myBox.getID(), true);
						else
							MessageManager.sendImDying(id, allyID, myBox.getID(), false);
					}
				}
				this.isFailed = true;
				Main.robotsFailed++;
				Logger.log(id + " FAIL PUSHING at round " + Main.rounds );
				return;
			} /** END FAILURE DURING PUSHING **/
			
			pushToGoal(myBox);
		}
		else /** DONE PUSHING - cleanup **/
		{
			/** box is in goal, never push it again **/			
			myBox.setIsInGoal();
						
			/** log **/
			if(isPushing)
				Logger.log("Robot " + id + " isPushing");
			if(isHelping)
				Logger.log("Robot " + id + " isHelping");
			if(isHelpArrived)
				Logger.log("Robot " + id + " isHelpArrived");
			if(isWaitingForHelp)
				Logger.log("Robot " + id + " isWaitingForHelp");
			
			/** log abigail**/
			Logger.log("Robot " + id + " done pushing " + myBox.getID());
			
			/** update success rate for the binding affinity **/
			Whiteboard.incrementSuccessRate(this.id + myBox.getID());
			
			/** reset values **/
			this.isPushing = false;
			this.isHelping = false;
			this.isHelpArrived = false;
			this.isWaitingForHelp = false;
			myBox = null;
			this.allyID = "";
						
			/** CLEAR message queue **/
			this.messageQueue.clear();
		}
	}
	
	/**
	 * Sonar-based obstacle avoidance
	 * @param a - the objects in the environment
	 */
	private void doObstacleAvoidance( ArrayList<EnvObjInterface> a)
	{
		/** SONAR BASED moves **/
		int ran = (int)(Main.getRandomNumber(id)*10);
		switch(sonar.getSonarValues(a))
		{
		case LEFT:
			turn(turnVelocity, RIGHT);
			if(ran < 5) moveForward(velocity, FORWARD);
			return;
		case RIGHT:
			turn(turnVelocity, LEFT);
			if(ran < 5) moveForward(velocity, FORWARD);
			return;
		case CENTER:
			if(ran < 3) turn(2*turnVelocity, LEFT);
			else turn(2*turnVelocity, RIGHT);
			moveForward(velocity, FORWARD);
			return;
		case RIGHTANDLEFT:
			moveForward(velocity, FORWARD);			
			if(ran < 3) turn(turnVelocity, LEFT);
			else turn(turnVelocity, RIGHT);
			return;
		case RIGHTANDCENTER:
			turn(turnVelocity, LEFT);
			if(ran < 7) moveForward(velocity, FORWARD);
			else moveForward(velocity, BACKWARD);
			return;
		case LEFTANDCENTER:
			turn(turnVelocity, RIGHT);
			if(ran < 7) moveForward(velocity, FORWARD);
			else moveForward(velocity, BACKWARD);
			return;
		case ALL:
			if(ran < 3) turn(2*turnVelocity, LEFT);
			else if(ran < 7) turn(2*turnVelocity, RIGHT);
			else turn(2*turnVelocity, RIGHT);
			ran = (int)(Main.getRandomNumber(id)*10);
			if(ran < 5) moveForward(velocity, BACKWARD);
			return;
		default:
			break;
		}
	}
	
	/**
	 * approach the designated object(detected box)
	 */
	private void doApproachingBox()
	{
		/** box reaches finish line already **/
		if(!detectedBox.isPushable())
		{
			detectedBox = null;
			
			this.allyID = "";
			this.isHelping = false;
			this.isPushing = false;
			
			if(isHelpArrived || isWaitingForHelp || myBox != null)
			{
				System.out.println("\nPOSSIBLE ERROR - doApproachingBox - not supposed to be in PUSHING MODE yet\n");
				if(isHelpArrived) System.out.println("isHelpArrived = true");
				if(isWaitingForHelp) System.out.println("isWaitingForHelp = true");
				if(myBox != null) System.out.println("myBox = " + myBox.getID());
			}
			
			this.isHelpArrived = false;
			this.isWaitingForHelp = false;
			myBox = null;
			
			return;
		}
		
		/** if the robot arrived at the location of the object **/
		if(isTouching(detectedBox))
		{
			if(this.isCapableToPush(detectedBox))
			{	
				this.isPushing = true;
				this.myBox = detectedBox;
				this.detectedBox = null;
				
				/** arrived to destination where it should help coordinator **/
				if(!this.allyID.equals(""))
				{
					MessageManager.sendArriveToHelpMessage(id, allyID, myBox.getID());
					isHelping = true;
				}
				
				/** otherwise, start the counter for coordination before pushing the box **/
				myBox.setStartCoordinationBeforePushing();
				
				return;
			}
			else /** not able to push the box! **/
			{
				/** RESET all the status! Add box to ignore list **/
				this.ignoreBoxes.add(detectedBox.getID());
				this.isPushing = false;
				this.isHelping = false;
				this.isWaitingForHelp = false;
				this.isHelpArrived = false;
				
				MessageManager.sendBoxNotifyMessage(id, detectedBox);
				this.detectedBox = null;
				return;
			}
		}
		else /** If we are still approaching the box, want to send WINNER msg continuously */
		{
			/** We know detectedBox != null because we are in doApproachingBox */
			if(!this.allyID.equals(""))
			{	
				/** FAILURE after winning AIS election **/
				//if(Main.getRandomNumber(id) > this.failChance[FailState.HELP.ordinal()])
				if( Main.RUN_FALSE_TOLERANCE_TESTS && 
						(detectedBox.getID().equals("b5") && test[2] && detectedBox.round1 > 0 && (Main.rounds - detectedBox.round1 > 20))
				  )
				{
					test[2] = false;
					/** 50 percent chance of partial failure **/
					if(Main.getRandomNumber(id + " CHECK PARTIAL FAILURE HELP ") < partialFailChance)
					{
						/** send dying message to ALLY **/
						MessageManager.sendImDying(id, allyID, detectedBox.getID(), true);
					}
					this.isFailed = true;
					Main.robotsFailed++;
					Logger.log(id + " FAIL HELP at round " + Main.rounds );
					return;
				} /** END FAILURE after winning AIS election **/
				
				Point myP = new Point((int)this.x, (int)this.y),
						boxP = new Point(detectedBox.getX(), detectedBox.getY());
				MessageManager.sendWinnerMessage(id, allyID, detectedBox.getID(), myP.distance(boxP));
			}
		}
		
		/** Calculate where the box is wrt this Robot **/
		double boxAngle = getGoalAngle(new Point(detectedBox.getX(), detectedBox.getY()));
		
		if(boxAngle > 340 && boxAngle < 20)
		{
			moveForward(velocity, FORWARD);
		}
		else if(boxAngle >= 20 && boxAngle < 180)
		{
			turn(turnVelocity, RIGHT);
			moveForward(velocity, FORWARD);
		}
		else
		{
			turn(turnVelocity, LEFT);
			moveForward(velocity, FORWARD);
		}
	}
	
	/**
	 * When a robot has crashed into another object
	 * @param a - the objects in the environment
	 */
	private void doCrashRecovery(ArrayList<EnvObjInterface> a)
	{
		int ran = (int)(Main.getRandomNumber(id)*100);
		/** BUMPER BASED moves **/
		/** SIDE BUMPERS ONLY **/
		if(this.leftSideBumperHit)
		{
			if(ran < 40)
				turn(turnVelocity, RIGHT);
			else if(ran < 80)
				turn(2*turnVelocity, RIGHT);
			/** 20% chance of not turning **/
		}
		if(this.rightSideBumperHit)
		{
			if(ran < 40)
				turn(turnVelocity, LEFT);
			else if(ran < 80)
				turn(2*turnVelocity, LEFT);
			/** 20% chance of not turning **/
		}
		
		/** FRONT BUMPERS **/
		if(this.frontBumperHit)
		{
			ran = (int)(Main.getRandomNumber(id)*100);
			if(ran < 40)
				turn(turnVelocity, LEFT);
			else if(ran < 80)
				turn(turnVelocity, RIGHT);
			moveForward(velocity, BACKWARD);
			return;
		}
		
		/** REAR BUMPERS **/
		if(this.leftRearBumperHit || this.rightRearBumperHit)
		{
			ran = (int)(Main.getRandomNumber(id)*100);
			if(ran < 40)
				turn(turnVelocity, LEFT);
			else if(ran < 80)
				turn(turnVelocity, RIGHT);
			moveForward(velocity, FORWARD);
			return;
		}
		
		return;		
	}
	
	/** Randomly navigate through the environment **/
	public void doRandomMovement()
	{
		int ran = (int)(Main.getRandomNumber(id)*200);
		if(ran < 20)
			turn(turnVelocity, RIGHT);
		else if(ran < 40 )
			turn(turnVelocity, LEFT);
		else if(ran < 200)
			moveForward(velocity, FORWARD);
	}
	
	/**
	 *  Main life cycle of the robot
	 * @param a - all the objects in the environment
	 */
	public void doSomething( ArrayList<EnvObjInterface> a)
	{
		/** if robot is dead, do nothing **/
		if(isFailed)
			return;
		/** first check if robot has any messages **/
		checkMessages(a);
		
		/** wait for other robots to send their affinity **/
		if(waitForCost > 0)
			waitForCost--;
		else if(isHelping && waitForCost == 0)
		{			
			/** wins bidding **/
			if(detectedBox != null)
				Logger.log("BID_WIN " + id + " will assist " + allyID + ", for box " + detectedBox.getID() + ", with cost " + this.myCost);
			else
				if(Main.DEBUG_MULTIPLE_ROBOTS_PUSHING) Main.pause = true;
			
			if(myBox != null)
			{
				Logger.log("BID_WIN_OVERRIDE " + id + " will assist " + allyID + ", for box " + myBox.getID());
				if(Main.DEBUG_MULTIPLE_ROBOTS_PUSHING) Main.pause = true;
			}
			
			/** wander, will have method to approach the box within **/
			wander(a);
			waitForCost--;
		}
		else
			wander(a);
	}
	
	/**
	 * Check the queue for any messages the robot should consider
	 * @param a - all the objects in the environment
	 */
	public void checkMessages(ArrayList<EnvObjInterface> a)
	{
		// if message queue not empty, process 1 message
		if(messageQueue.isEmpty())
			return;
		else
		{
			//Logger.log("Queue " + id + " size: " + messageQueue.size());
		}
		
		Message m = messageQueue.removeFirst();
		
		/** if force point is the same, ignore message**/
		if(m.getMessageType() == MessageType.GO_TO_FORCE_POINT &&
				m.getForcePoint() == this.currentAction)
		{
			/** IGNORE Log messages for repetitive GO_TO_FORCE_POINT messages **/
		}
		else if(m.getMessageType() == MessageType.COST)
			Logger.log("HANDLING " + id + " m: " + m);
		else
			Logger.log("HANDLING " + id + " m: " + m);
		
		/** handle the different types of messages with these methods **/
		switch(m.getMessageType())
		{
		case HELP: handleHelpMessage(m, a); break;
		case BOX_IGNORE: handleBoxIgnoreMessage(m, a); break;
		case BOX_NOTIFY: handleBoxNotifyMessage(m, a); break;
		case RESPOND_TO_HELP: handleRespondHelpMessage(m, a); break;
		case ARRIVE_TO_HELP: handleArriveToHelpMessage(m, a); break;
		case I_AM_THE_WINNER: handleWinnerMessage(m, a); break;
		case I_AM_ALIVE: handleImAliveMessage(m, a); break;
		case I_AM_DYING: handleIAmDyingMessage(m, a); break;
		case GO_TO_FORCE_POINT: handleGoToForcePointMessage(m, a); break;
		case COST: handleCostMessage(m,a); checkMessages(a); break;
		default: 
			System.err.println("UNKNOWN MESSAGE TYPE: " + m.getMessageType());
			break;
		}
	}
	
	/**
	 * 
	 * @param m
	 * @param a
	 */
	public void handleWinnerMessage(Message m, ArrayList<EnvObjInterface> a )
	{
		if(isWaitingForHelp && myBox != null && m.getBoxID().equals(myBox.getID())
				&& lastWinnerMessage != m.getDist())
		{			
			waitCount = 1;
			lastWinnerMessage = m.getDist();
			
			/** We want to set ally to the first winner, therefore, if another robot comes before the winner does, it will tell it to go away**/
			allyID = m.getSenderID();
		}		
	}
	
	public void handleIAmDyingMessage(Message m, ArrayList<EnvObjInterface> a )
	{
		if( (myBox != null && m.getBoxID().equals(myBox.getID())) )
		{			
			this.waitForForcePointMsg = 0;
			this.previousLocCounter = 0;
			this.allyID = "";
			this.isHelpArrived = false;
			this.isHelping = false;
			this.isWaitingForHelp = true;
			if(myBox != null)
				myBox.isBeingPushed = false;
//				else if(detectedBox != null)
//					detectedBox.isBeingPushed = false;
			waitCount = WAIT_COUNT_THRESHOLD;
			while(!this.messageQueue.isEmpty())
			{
				Message m2 = this.messageQueue.remove();
				Logger.log("REMOVING_MESSAGE " + id + " m: " + m2);
			}
			Logger.log("QUEUE_EMPTY " + id);
		}

		else if( m.getSenderID().equals(allyID) )
		{			
			this.waitForForcePointMsg = 0;
			this.previousLocCounter = 0;
			this.allyID = "";
			this.isHelpArrived = false;
			this.isHelping = false;
			this.isWaitingForHelp = false;
			if(detectedBox != null)
				detectedBox.isBeingPushed = false;
			waitCount = WAIT_COUNT_THRESHOLD;
			while(!this.messageQueue.isEmpty())
			{
				Message m2 = this.messageQueue.remove();
				Logger.log("REMOVING_MESSAGE " + id + " m: " + m2);
			}
			Logger.log("QUEUE_EMPTY " + id);
		}
	}
	
	public void handleHelpMessage(Message m, ArrayList<EnvObjInterface> a)
	{
		
		if(waitForCost > 0)
		{
			//System.err.println("Robot " + id + " is waiting for cost. Drop: " + m);
			Logger.log("DROP_HELP " + id + " is waiting for cost. Drop: " + m);
			return;
		}
		
		if( isPushing && !m.getBoxID().equals(myBox.getID()))
		{
			//System.err.println("Robot " + id + " is pushing: " + myBox.getID() + ". Drop: " + m);
			Logger.log("DROP_HELP " + id + " is pushing: " + myBox.getID() + ". Drop: " + m);
			return;
		}
		
		if(isHelping)
		{
			//System.err.println("Robot " + id + " is helping: " + allyID + ". Drop: " + m);
			Logger.log("DROP_HELP " + id + " is helping: " + allyID + ". Drop: " + m);
			return;
		}
		
		Box b = null;
		for(EnvObjInterface obj : a)
		{
			if(obj.getID().equals(m.getBoxID()) && obj.isPushable())
			{
				b = (Box) obj;
				if(ignoreBoxes.contains(obj.getID()))
				{
					Logger.log("Robot " + id + " is ignoring box " + obj.getID());
					return;
				}
				if(!this.isCapableToPush(b))
				{
					ignoreBoxes.add(b.getID());
					Logger.log("Robot " + id + " is not capable to push, ignoring box " + obj.getID());
					return;
				}
					
				this.detectedBox = (Box) obj;
				this.allyID = m.getCoordinatorID();
				this.isHelping =true;
				this.isPushing = false;
				this.isWaitingForHelp = false;
				break;
			}
		}
		
		Logger.log("BID " + id + " is in the bidding to assist " + allyID + " for " + b.getID() );
		
		/** Calculate then broadcast COST **/
		int cost = RobotHelper.calculateBindingAffinity(this, b);
		MessageManager.sendCostMessage(id, m.getCoordinatorID(), m.getBoxID(), cost);
		
		this.waitForCost = 10;
		this.myCost = cost;
	}

	public void handleBoxIgnoreMessage(Message m, ArrayList<EnvObjInterface> a)
	{
		/**
		 * r1 START BID for box
		 * 
		 * r4 WINS BID
		 * 
		 * r2 gets to the box first
		 * - check if there is any other robots WAITING FOR HELP to push the SAME BOX
		 * - if there isn't, do an election
		 * 
		 * CHANGES:
		 * - Don't start a new election, instead send ArriveToHelp if sbd else is already at the box
		 * - Change handleArriveToHelp:
		 *   - check for if isHelpArrived=true, then send new msg BOX_IGNORE to sender
		 *   
		 * - ADD handleBoxIgnore:
		 *   - Remove detectedBox, myBox, allyID, isHelping=false, isPushing=false
		 *   - and if the robot still touches the box, it will check that the box is actually being pushed and avoid it
		 */
		
		/** Need to check for same boxID ?? **/
		detectedBox = null;
		myBox = null;
		allyID = "";
		this.isHelpArrived = false;
		this.isHelping = false;
		this.isPushing = false;
		this.isWaitingForHelp = false;
		this.angle = (this.angle + 180) % 360;
	}
	
	public void handleBoxNotifyMessage(Message m, ArrayList<EnvObjInterface> a)
	{
		if(waitForCost > 0)
		{
			//System.err.println("Robot " + id + " is waiting for cost. Drop: " + m);
			Logger.log("DROP_NOTIFY " + id + " is waiting for cost. Drop: " + m);
			return;
		}
		
		if( isPushing && !m.getBoxID().equals(myBox.getID()))
		{
			//System.err.println("Robot " + id + " is pushing: " + myBox.getID() + ". Drop: " + m);
			Logger.log("DROP_NOTIFY " + id + " is pushing: " + myBox.getID() + ". Drop: " + m);
			return;
		}
		
		if(isHelping)
		{
			//System.err.println("Robot " + id + " is helping: " + allyID + ". Drop: " + m);
			Logger.log("DROP_NOTIFY " + id + " is helping: " + allyID + ". Drop: " + m);
			return;
		}
		
		/** If currently approaching another box, ignore the message **/
		if(detectedBox != null)
		{
			if(!m.getBoxID().equals(detectedBox.getID()))
			{
				Logger.log("DROP_NOTIFY " + id + " already approaches: " + detectedBox.getID() + ". Drop: " + m);
				return;
			}
			else
			{
				Logger.log("\nPOSSIBLE_PROBLEM:  " + id + " already approaches: " + detectedBox.getID() + ". Drop: " + m + "\n");
				//Main.pause = true;
			}
		}		
		
		Box b = null;
		for(EnvObjInterface obj : a)
		{
			if(obj.getID().equals(m.getBoxID()) && obj.isPushable())
			{
				b = (Box) obj;
				
				if(ignoreBoxes.contains(obj.getID()))
				{
					Logger.log("Robot " + id + " is ignoring box " + obj.getID());
					return;
				}
				if(!this.isCapableToPush(b))
				{
					ignoreBoxes.add(b.getID());
					Logger.log("Robot " + id + " is not capable to push, ignoring box " + obj.getID());
					return;
				}
				
				this.detectedBox = (Box) obj;
				this.isHelping = false;
				/** No coordinator yet!, approach the box **/
				break;
			}
		}
		
		/** Not sure whether or not we need to start an auction for the box **/
		/** we do for HELP messages, but what if we don't have a coordinator as in BOX_NOTIFY message? **/
		
		/** trying to auction **/
		Logger.log("Robot " + id + " is in the bidding to assist " + allyID + " for " + b.getID() );
		
		/** Calculate then broadcast COST **/
		int cost = RobotHelper.calculateBindingAffinity(this, b);
		MessageManager.sendCostMessage(id, "", m.getBoxID(), cost);
		
		this.waitForCost = 10;
		this.myCost = cost;
	}
	
	public void handleRespondHelpMessage(Message m, ArrayList<EnvObjInterface> a)
	{		
		if(waitForCost > 0)
		{
			Logger.log("DROP_RESPOND_HELP " + id + " is waiting for cost. Drop: " + m);
			return;
		}
		this.isWaitingForHelp = true;
	}
	
	public void handleCostMessage(Message m, ArrayList<EnvObjInterface> a)
	{		
		if(!m.getCoordinatorID().equals("") && 
				isHelping && detectedBox != null && detectedBox.getID().equals(m.getBoxID()) &&
				m.getCoordinatorID().equals(allyID))
		{
			int ba = m.getCost();
			/** if someone has higher ba, then we go to wander **/
			if(ba >= this.myCost)
			{
				Logger.log("BID_LOST " + id + " is NOT HELPING " + allyID + " due to " + m.getSenderID());
				this.isHelping = false;
				this.allyID = "";
				this.detectedBox = null;
				this.myCost = 0;
				this.waitForCost = 0;
			}
			else
			{
				return;
			}
		}
		/** COST message with no coordinator, this is for auction when receiving BOX_NOTIFY **/
		else if(m.getCoordinatorID().equals("") && 
				detectedBox != null && detectedBox.getID().equals(m.getBoxID()))
		{
			int ba = m.getCost();
			/** if someone has higher ba, then we go to wander **/
			if(ba >= this.myCost)
			{
				Logger.log("BID_LOST " + id + " is NOT APPROACHING " + detectedBox.getID() + " due to " + m.getSenderID());
				this.isHelping = false;
				this.allyID = "";
				this.detectedBox = null;
				this.myCost = 0;
				this.waitForCost = 0;
			}
			else
			{
				/** bid is still alive **/
				return;
			}
		}
	}
	
	public void handleArriveToHelpMessage(Message m, ArrayList<EnvObjInterface> a)
	{
		/** Hmm, this sounds like a bad idea, comment it for now
		if(waitForCost > 0)
		{
			Logger.log("Robot " + id + " is waiting for cost. Drop: " + m);
			return;
		}
		*/
		if((allyID != null) && !allyID.equals("") && !allyID.equals(m.getSenderID()))
		{
			if(isHelpArrived)
				Logger.log("\nTOO MANY ALLIES: coord " + id + " allies with " + allyID + "\n");
			else if(isHelping)
				Logger.log("\nTOO MANY ALLIES: helper " + id + " allies with " + allyID + "\n");
			
			MessageManager.sendBoxIgnoreMessage(id, m.getSenderID(), m.getBoxID());
			return;
		}
		
		this.isWaitingForHelp = false;
		this.isPushing = true;
		this.myBox.isBeingPushed = true;
		this.isWaitingForAllyAliveMsg = false;
		/** TOAR: this might be key! **/
		//this.isHelping = false;
		
		this.isHelpArrived = true;
		this.allyID = m.getSenderID();
		
	}
	
	public void handleGoToForcePointMessage(Message m, ArrayList<EnvObjInterface> a)
	{
		if(waitForCost > 0)
		{
			Logger.log("Robot " + id + " is waiting for cost. Drop: " + m);
			return;
		}
		/** the coordinator must be equal to the allyID **/
		if(!m.getCoordinatorID().equals(allyID))
		{
			System.err.println("ERROR in COORDINATION: " + id + ", real allyID: " + allyID + ", msg: " + m);
			Logger.log("ERROR in COORDINATION: " + id + ", real allyID: " + allyID + ", msg: " + m);
			return;//System.exit(1);
		}
		
		/** must be in isHelping mode **/
		if(!this.isHelping)
		{
			System.err.println("ERROR in COORDINATION: " + id + ", allyID: " + allyID + ", isHelping is false");
			Logger.log("ERROR in COORDINATION: " + id + ", allyID: " + allyID + ", isHelping is false");
			return;//System.exit(1);
		}		
		
		
		
		/** Get the assigned Force Point */
		currentAction = m.getForcePoint();
		
		/** TODO: FAILING ROBOTS **/
//		/** FAILED ALLIES - NOT COORD!!! **/
//		if(Main.getRandomNumber(id) > .9 && !id.equals("r2") && !id.equals("r3") && !id.equals("r5"))// && !id.equals("r2"))
//		{
//			Logger.log("FAIL  --- " + this.id);
//			this.isFailed = true;
//		}
		
		/**
		 * Check if there is a robot(most likely dead) occupying the assigned
		 * Force Point in which case we choose another Force Point to push from.
		 */
		if(PointIsInADeadRobot(myBox.getForcePoint(currentAction)) || this.step_stamp > 0)
		{
			if(this.step_stamp == 0) this.step_stamp = 20;
			this.step_stamp--;
			
			if(currentAction == 1 || currentAction == 2)
			{
				currentAction = 6;
			}
			else if(currentAction == 5 || currentAction == 4)
			{
				currentAction = 7;
			}			
		}		
	}
	
	public void handleImAliveMessage(Message m, ArrayList<EnvObjInterface> a)
	{		
		if(m.getSenderID().equals(this.allyID))
		{
			Logger.log(m.getSenderID() + " IS ALIVE, continue.");
			this.isWaitingForAllyAliveMsg = false;
			this.WaitForAllyAliveMsg = 0;
		}
	}
	
	/**
	 * Private helper method to check if a certain Force Point
	 * is occupied by another robot. 
	 * @param p - the Force Point in question
	 * @return true - if there is a robot there
	 * 			false - otherwise
	 */
	private boolean PointIsInADeadRobot(Point p)
	{
		for(EnvObjInterface obj : Main.a)
		{
			if(obj.getID() != this.id && 
				obj.getID().startsWith("r") && ((Robot) obj).isFailed)
			{
				Robot r = (Robot) obj;
				if(r.getBounds().contains(p)) return true;
			}
		}
		return false;
	}
	
	public void wander(ArrayList<EnvObjInterface> a)
	{
		/** pushing a box **/
		if( isPushing )
		{
			doPushing();
			return;
		}
		
		/** crash recovery **/
		/** NOTE: Possible to crash at the back too? **/
		if(!validateRobotPoints(a))
		{		
			doCrashRecovery(a);
			return;
		}
		
		/** obstacle avoidance **/
		if(isObstacleDetected(a))
		{
			doObstacleAvoidance(a);
			return;
		}
		
		/** detect a box **/
		/** the second condition might be valid when receiving help msg **/
		if( detectedBox != null || isBoxDetected(a) )
		{
			/** check itself whether or not goal is known **/
			if(goalKnown)
			{
				doApproachingBox();
				return;
			}
			/** otherwise, check whiteboard for goal **/
			else if(Whiteboard.goalKnown)
			{
				goalKnown = true;
				doApproachingBox();
				return;
			}
			/** otherwise, continue to wander **/
		}
		
		if(!goalKnown && this.goal.contains(this))
		{
			goalKnown = true;
			Whiteboard.goalKnown = true;
			System.out.println(id + " FINDS GOAL");
		}
		
		/** FAILURE DURING SEARCH - ONLY DURING SEARCH**/
		if(Main.getRandomNumber(id) > this.failChance[FailState.WANDER.ordinal()])
		{
			/** 50 percent chance of partial failure **/
			if(Main.getRandomNumber(id + " CHECK PARTIAL FAILURE SEARCH ") > partialFailChance)
			{
				/** do nothing for search **/
			}
			this.isFailed = true;
			Main.robotsFailed++;
			Logger.log(id + " FAIL SEARCH at round " + Main.rounds );
			return;
		}
		
		/** randomly move **/
		doRandomMovement();
	}
	
	public void moveForward(int howMany, int direction)
	{
		int forceValue[] = new int[8];
		for(int i=0; i<8;i++)
			forceValue[i] = 0;
		forceValue[direction] = howMany;
		moveRobot(forceValue);
	}
	
	public void turn(int howMany, int direction)
	{
		int forceValue[] = new int[8];
		for(int i=0; i<8;i++)
			forceValue[i] = 0;
		forceValue[direction] = howMany;
		turnRobot(forceValue);
	}
	
	/**
	 * Helper for moving
	 */
	private void moveRobot(int [] forceValue)
    {
		double lArm=20;  // force arm
        double Mf=0.2; // Force-Displacement factor
        double Mt=0.02; //Torque-Angle factor  
        
		double fx = forceValue[0] - forceValue[3];
        double fy = forceValue[1] + forceValue[2] + forceValue[6] - forceValue[4] - forceValue[5] - forceValue[7];
        double netForceAngle = (Math.atan2(fy,fx)) / Math.PI * 180; //local coordinate
        double netForceValue = Math.sqrt( fx*fx + fy*fy );
        double netTorque = (forceValue[5] + forceValue[2] - forceValue[1] - forceValue[4]) * lArm;
        
        double newX = Math.round(x+Mf*netForceValue*Math.cos((netForceAngle+angle)/180*Math.PI));  // global coordinate
        double newY = Math.round(y+Mf*netForceValue*Math.sin((netForceAngle+angle)/180*Math.PI));  // global coordinate
        double newAngle = angle+netTorque*Mt;
        angle = newAngle % 360.0;
        x = newX;
        y = newY;
        this.bounds.setBounds((int)(x - width/2), (int) (y - height/2), width, height);
    }
	
	/**
	 * Applying torque to turn the robot, no x,y movement!
	 * @param forceValue
	 */
	private void turnRobot(int [] forceValue)
    {
		double lArm=20;  // force arm
        double Mt=0.02; //Torque-Angle factor
        
        double netTorque = (forceValue[5] + forceValue[2] - forceValue[1] - forceValue[4]) * lArm;
        
        angle = angle+netTorque*Mt;
        angle = angle % 360.0;
    }
	
	/** helper for move towards box **/
	private double getGoalAngle(Point goal)
	{
        Point goalLocalPosition=new Point(0,0);
        frameConvert(goal, goalLocalPosition, new Point(getX(),getY()), angle);
        double goalAngle=(Math.atan2(goalLocalPosition.y, goalLocalPosition.x))/Math.PI*180;
        goalAngle += 90;
        while(goalAngle<0)
            goalAngle=goalAngle+360;
		return goalAngle;
	}
	
    private void frameConvert(Point p1, Point p2, Point origin, double angle){  // convert the global coordinate into local coordinate
        p2.x=(int) (p1.x*Math.cos(angle/180*Math.PI)+p1.y*Math.sin(angle/180*Math.PI)
             -origin.x*Math.cos(angle/180*Math.PI)-origin.y*Math.sin(angle/180*Math.PI));
        p2.y=(int) (p1.y*Math.cos(angle/180*Math.PI)-p1.x*Math.sin(angle/180*Math.PI)
             +origin.x*Math.sin(angle/180*Math.PI)-origin.y*Math.cos(angle/180*Math.PI));
    }  
	
	
	/** CAPABILITES FUNCTIONS **/
	public void setCap(boolean maxPayload, boolean arm, boolean mobility, boolean sensors, boolean gripper)
	{
		this.myCapabilities = new Capabilities(maxPayload, arm, mobility, sensors, gripper);	
	}
	
	public void setCap(Capabilities cap)
	{
		this.myCapabilities = cap;
	}
	
	public Capabilities getCapabilities()
	{
		return this.myCapabilities;
	}
	
	public boolean isCapableToPush(Box b)
	{
		return this.myCapabilities.contains(b.getCapabilities());
	}
	
	/** MESSAGES PART **/
	
	/**
	 * Receives a message from other robot, add to message queue
	 * When in PUSHING state, ignore all messages from NON-ALLY
	 * @param m
	 * @return
	 */
	public boolean receiveMessage(Message m)
	{
		/** IGNORE messages from non ALLY when in pushing state 
		/** Do not ignore Arrive_to_help, Are_you_alive, I_am_alive and cost messages **/
		if(isPushing && m.getMessageType() != MessageType.COST && m.getMessageType() != MessageType.ARRIVE_TO_HELP
				&& m.getMessageType() != MessageType.ARE_YOU_ALIVE && m.getMessageType() != MessageType.I_AM_ALIVE 
				&& m.getMessageType() != MessageType.I_AM_DYING)
		{
			if(!allyID.equals("") && !m.getSenderID().equals(this.allyID))
			{
				Logger.log("DROP " + id + " assisting " + this.allyID + ", drops: " + m);
				return false;
			}
		}
		
		if(m.getMessageType() == MessageType.GO_TO_FORCE_POINT &&
				m.getForcePoint() == this.currentAction)
		{
			/** IGNORE Log messages for repetitive GO_TO_FORCE_POINT messages **/
			messageQueue.addLast(m);
			return true;
		}
		
		Logger.log("RECV " + id + ": " + m);
		messageQueue.addLast(m);
		return true;
	}
	
	public void receiveAliveMessage(Message m)
	{
		if(this.isFailed)
			return;
		else
		{		
			if(myBox != null)
				MessageManager.sendImAlive(this.id, m.getSenderID(), myBox.getID(), false);
			else if(detectedBox != null)
				MessageManager.sendImAlive(this.id, m.getSenderID(), detectedBox.getID(), true);
		}
	}
	
	public boolean askForHelp(Box b)
	{
		/** should not always ask for help... */
		if(this.isHelpArrived || this.isHelping) 
			return false;
		else
		{
			/** Sends help, START ARTIFICIAL IMMUNE AUCTION **/
			if(this.isWaitingForHelp)
			{
				if(waitCount % WAIT_COUNT_THRESHOLD == 0)
				{
					allyID = "";
					MessageManager.sendHelpMessage(this, b);
				}
				waitCount++;
			}
			else
			{
				if(!boxNotPushedYet(b.getID()))
				{
					/** ARRIVE_TO_HELP message is sent within boxNotPushedYet **/
					/** Robot's own states are updated within the method as well **/
					if(Main.DEBUG_MULTIPLE_ROBOTS_PUSHING) Main.pause = true;
					return false;
				}
				this.allyID = "";
				MessageManager.sendHelpMessage(this, b);
				this.isWaitingForHelp = true;
				waitCount = 1;
				return true;
			}
		}
		return false;
	}
	
	private boolean boxNotPushedYet(String boxID)
	{
		boolean ret = true;
		for(EnvObjInterface obj : Main.a)
		{
			/** look for robots **/
			if(obj.getID().startsWith("r") && !obj.getID().equals(id))
			{
				Robot r = (Robot)obj;
				if(!r.isFailed && r.myBox != null && r.myBox.getID().equals(boxID))
				{
					Logger.log("");
					/** note that ally hasn't come yet, allyID = "" **/
					Logger.log("\tPOSSIBLE_PROBLEM: " + boxID + ", is already pushed by: " + r.getID());
					Logger.log("\tPOSSIBLE_PROBLEM: " + boxID + ", is trying to be pushed by: " + this.id);
					Logger.log("Sending arrive to help");
					
					/** Send arrive to help **/
					MessageManager.sendArriveToHelpMessage(id, r.getID(), boxID);
					this.allyID = r.getID();
					this.isPushing = true;
					this.isHelping = true;
					this.isHelpArrived = false;
					this.isWaitingForHelp = false;
					ret = false;
				}
			}
		}
		return ret;
	}
}